#include "main.h"

#define FRAME_CNT 5000
#define RX_TIMEOUT 30
#define BUF_LEN 64
int main(int argc, char* argv[])
{
	const char dev[] = "COM16";
	if (connect_serial(dev) == -1)
	{
		printf("serial open error!\n");
		return -1;
	}
	Odom odom;
	uint8_t buf[BUF_LEN];
	uint8_t tmp[BUF_LEN];
	uint32_t s_len = 0;
	for (int ii = 0; ii < FRAME_CNT; ii++)
	{
		s_len += read_serial(buf + s_len, BUF_LEN - s_len, RX_TIMEOUT);
		if (s_len >= ODOM_SIZE())
		{
			int i = 0, j = 0;
			for (i = 0; i < s_len - ODOM_SIZE();)
			{
				if (Odom_Pop(&odom, buf + i))
				{
					printf("px=%6.3f,py=%6.3f,pz=%6.3f,vx=%6.3f,vy=%6.3f,vz=%6.3f\n", odom.px / 1000.f, odom.py / 1000.f, odom.pz / 1000.f, odom.vx / 1000.f, odom.vy / 1000.f, odom.vz / 1000.f);
					j = i;
					i += ODOM_SIZE();
				}
				else
				{
					j = i;
					i++;
				}
			}
			uint32_t n_len = s_len - j;
			memcpy(tmp, buf+j, n_len);
			memcpy(buf, tmp, n_len);
			s_len = n_len;
			
		}
	}
	disconnect_serial();
	return 0;
}